#include "ros/ros.h"
#include "iostream"
#include "image_transport/image_transport.h"
#include "cv_bridge/cv_bridge.h"
#include "sensor_msgs/image_encodings.h"
#include "camera_info_manager/camera_info_manager.h"

#include "opencv2/opencv.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "ImageCatcher.h"

using namespace std;

#ifndef IMAGE_PUBLISHER_H
#define IMAGE_PUBLISHER_H

namespace ImagePublisher {
	typedef struct ImageTopics {
		const char *originImageTopic, *leftImageTopic, *rightImageTopic;
	} ImageTopics;

	class ImagePublisher {
	private:
		cv::Mat origin, leftImage, rightImage;
		image_transport::ImageTransport imageTransport;
		image_transport::CameraPublisher leftPublisher, rightPublisher;
		image_transport::Publisher imagePublisher;
		cv_bridge::CvImagePtr leftImagePtr = nullptr;
		cv_bridge::CvImagePtr rightImagePtr = nullptr;
		boost::shared_ptr<camera_info_manager::CameraInfoManager> cameraInfoManager;
		sensor_msgs::CameraInfoPtr leftCameraInfoPtr, rightCameraInfoPtr;
		image_transport::Subscriber imageSubscriber;

	public:
		explicit ImagePublisher(const ros::NodeHandle &nodeHandle, ImageTopics imageTopics);
		bool setNewFrame(const cv::Mat &frame);
		void split(cv::Mat &left, cv::Mat &right);
		void split();
		void publish(const ImageCatcher& imageCatcher);
		void publish(const char* compositeTopic);
		sensor_msgs::ImagePtr publishComposite(ImageCatcher imageCatcher);
		void imageCallback(const sensor_msgs::ImageConstPtr &msg);
	};

	ImagePublisher::ImagePublisher(const ros::NodeHandle &nodeHandle, ImageTopics imageTopics) : imageTransport(nodeHandle) {
		imagePublisher = imageTransport.advertise(imageTopics.originImageTopic, 1);
		leftPublisher = imageTransport.advertiseCamera(imageTopics.leftImageTopic, 1);
		rightPublisher = imageTransport.advertiseCamera(imageTopics.rightImageTopic, 1);
		cameraInfoManager = boost::shared_ptr<camera_info_manager::CameraInfoManager>(new camera_info_manager::CameraInfoManager(nodeHandle));

//	读取服务器参数，获得左右相机参数文件位置
		string leftCalFile = nodeHandle.param<std::string>("left_cam_file", "");
		string rightCalFile = nodeHandle.param<std::string>("right_cam_file", "");
		if (!leftCalFile.empty()) {
			if (cameraInfoManager->validateURL(leftCalFile)) {
				cout << "Load left camera info file: " << leftCalFile << endl;
				cameraInfoManager->loadCameraInfo(leftCalFile);
				leftCameraInfoPtr = sensor_msgs::CameraInfoPtr(
						new sensor_msgs::CameraInfo(cameraInfoManager->getCameraInfo()));
			} else {
				cout << "Can't load left camera info of file: " << leftCalFile << endl;
				ros::shutdown();
			}
		} else {
			cout << "Did not specify left camera info file. " << endl;
			leftCameraInfoPtr = sensor_msgs::CameraInfoPtr(new sensor_msgs::CameraInfo());
		}
		if (!rightCalFile.empty()) {
			if (cameraInfoManager->validateURL(rightCalFile)) {
				cout << "Load right camera info file: " << rightCalFile << endl;
				cameraInfoManager->loadCameraInfo(rightCalFile);
				rightCameraInfoPtr = sensor_msgs::CameraInfoPtr(
						new sensor_msgs::CameraInfo(cameraInfoManager->getCameraInfo()));
			} else {
				cout << "Can't load right camera info of file: " << rightCalFile << endl;
				ros::shutdown();
			}
		} else {
			cout << "Did not specify right camera info file. " << endl;
			rightCameraInfoPtr = sensor_msgs::CameraInfoPtr(new sensor_msgs::CameraInfo());
		}

	}

	void ImagePublisher::split(cv::Mat &left, cv::Mat &right) {
		if (origin.empty()) cout << "in func split(): split origin empty." << endl;
		int midCol = origin.cols / 2;
		int height = origin.rows;
		left = origin(cv::Rect(0, 0, midCol, height));
		right = origin(cv::Rect(midCol, 0, midCol, height));
	}

	void ImagePublisher::split() {
		split(leftImage, rightImage);
	}

	bool ImagePublisher::setNewFrame(const cv::Mat &frame) {
		origin = frame.clone();
		return !origin.empty();
	}

	sensor_msgs::ImagePtr ImagePublisher::publishComposite(ImageCatcher imageCatcher) {
		setNewFrame(imageCatcher.getFrame());
		sensor_msgs::ImagePtr imagePtr = cv_bridge::CvImage(std_msgs::Header(), "bgr8", origin).toImageMsg();
		imagePublisher.publish(imagePtr);
		return imagePtr;
	}

	void ImagePublisher::publish(const ImageCatcher& imageCatcher) {
		sensor_msgs::ImagePtr imagePtr = publishComposite(imageCatcher);
		split();
		leftImagePtr = cv_bridge::CvImagePtr(new cv_bridge::CvImage(std_msgs::Header(), "bgr8", leftImage));
		rightImagePtr = cv_bridge::CvImagePtr(new cv_bridge::CvImage(std_msgs::Header(), "bgr8", rightImage));
		cv_bridge::CvImageConstPtr cv_ptr;
		cv_ptr = cv_bridge::toCvShare(imagePtr, "bgr8");
		leftCameraInfoPtr->header = cv_ptr->header;
		rightCameraInfoPtr->header = cv_ptr->header;
		sensor_msgs::ImagePtr leftPtr = leftImagePtr->toImageMsg();
		sensor_msgs::ImagePtr rightPtr = rightImagePtr->toImageMsg();
		leftPtr->header = imagePtr->header;
		rightPtr->header = imagePtr->header;
		leftPublisher.publish(leftPtr, leftCameraInfoPtr);
		rightPublisher.publish(rightPtr, rightCameraInfoPtr);
	}

	void ImagePublisher::publish(const char* compositeTopic) {
		imageSubscriber = imageTransport.subscribe(compositeTopic, 1, &ImagePublisher::imageCallback, this);
		cout << "in func publish(compositeTopic): " << "subscribed composite topic." << endl;
	}

	void ImagePublisher::imageCallback(const sensor_msgs::ImageConstPtr &msg) {
		cv_bridge::CvImageConstPtr cv_ptr;
		cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
		if (cv_ptr->image.empty()) cout << "in imageCallback(): msg image empty" << endl;
		this->origin = cv_ptr->image;
		split();
		leftImagePtr = cv_bridge::CvImagePtr(new cv_bridge::CvImage(std_msgs::Header(), "bgr8", leftImage));
		rightImagePtr = cv_bridge::CvImagePtr(new cv_bridge::CvImage(std_msgs::Header(), "bgr8", rightImage));
		leftCameraInfoPtr->header = cv_ptr->header;
		rightCameraInfoPtr->header = cv_ptr->header;
		sensor_msgs::ImagePtr leftPtr = leftImagePtr->toImageMsg();
		sensor_msgs::ImagePtr rightPtr = rightImagePtr->toImageMsg();
		leftPtr->header = msg->header;
		rightPtr->header = msg->header;
		leftPublisher.publish(leftPtr, leftCameraInfoPtr);
		rightPublisher.publish(rightPtr, rightCameraInfoPtr);
		cv::imshow("origin", origin);
	}
}
#endif //IMAGE_PUBLISHER_H